#include <opencv2/opencv.hpp>
#include <iostream>
#include <queue>
#include <vector>
#include <algorithm>
#include <set>

using namespace cv;
using namespace std;

// -------------------- 配置
vector<uchar> freeSpace = {255,220};
vector<uchar> bridgeCenterValues = {32,120,140};

// -------------------- 通行判断
bool isFreeForRegion(int x,int y,const Mat& img){
    uchar v = img.at<uchar>(y,x);
    return find(freeSpace.begin(), freeSpace.end(), v) != freeSpace.end();
}

// -------------------- 区域分割
Mat regionSegmentation(const Mat& img, Mat& regionId, int& regionCount, vector<Vec3b>& colors){
    regionId = Mat(img.size(), CV_32S, Scalar(-1));
    regionCount = 0;
    vector<Point> dirs = {{1,0},{-1,0},{0,1},{0,-1}};
    RNG rng(12345);

    for(int y=0;y<img.rows;y++){
        for(int x=0;x<img.cols;x++){
            if(regionId.at<int>(y,x)!=-1) continue;
            if(!isFreeForRegion(x,y,img)) continue;

            queue<Point> q;
            q.push(Point(x,y));
            regionId.at<int>(y,x) = regionCount;

            while(!q.empty()){
                Point p = q.front(); q.pop();
                for(auto d:dirs){
                    int nx=p.x+d.x, ny=p.y+d.y;
                    if(nx<0||ny<0||nx>=img.cols||ny>=img.rows) continue;
                    if(regionId.at<int>(ny,nx)==-1 && isFreeForRegion(nx,ny,img)){
                        regionId.at<int>(ny,nx)=regionCount;
                        q.push(Point(nx,ny));
                    }
                }
            }
            // 随机颜色
            colors.push_back(Vec3b(rng.uniform(50,255),rng.uniform(50,255),rng.uniform(50,255)));
            regionCount++;
        }
    }

    // 可视化
    Mat vis(img.size(), CV_8UC3);
    for(int y=0;y<img.rows;y++){
        for(int x=0;x<img.cols;x++){
            int id = regionId.at<int>(y,x);
            if(id>=0) vis.at<Vec3b>(y,x)=colors[id];
            else {
                uchar v = img.at<uchar>(y,x);
                vis.at<Vec3b>(y,x)=Vec3b(v,v,v); // 障碍物保持灰度
            }
        }
    }

    cout<<"区域数量: "<<regionCount<<endl;
    return vis;
}

// -------------------- 桥中心结构
struct BridgeCenter{
    vector<Point> line; // 整条桥中心
    int regionA, regionB;
    uchar value;
    Point startPoint; // 桥的起点（在区域A中）
    Point endPoint;   // 桥的终点（在区域B中）
};

// -------------------- 桥中心检测 (支持 L 型 / 弯曲桥)
vector<BridgeCenter> detectBridges(const Mat& img, const Mat& regionId){
    vector<BridgeCenter> bridges;
    Mat visited = Mat::zeros(img.size(), CV_8U);

    vector<Point> dirs8 = {{1,0},{-1,0},{0,1},{0,-1},{1,1},{1,-1},{-1,1},{-1,-1}};
    vector<Point> dirs4 = {{1,0},{-1,0},{0,1},{0,-1}};

    for(int y=0;y<img.rows;y++){
        for(int x=0;x<img.cols;x++){
            if(visited.at<uchar>(y,x)) continue;
            uchar v = img.at<uchar>(y,x);
            if(find(bridgeCenterValues.begin(), bridgeCenterValues.end(), v) == bridgeCenterValues.end())
                continue;

            vector<Point> line;
            queue<Point> q;
            q.push(Point(x,y));
            visited.at<uchar>(y,x) = 1;

            while(!q.empty()){
                Point p = q.front(); q.pop();
                line.push_back(p);

                for(auto d:dirs8){
                    int nx=p.x+d.x, ny=p.y+d.y;
                    if(nx<0||ny<0||nx>=img.cols||ny>=img.rows) continue;
                    if(visited.at<uchar>(ny,nx)) continue;
                    uchar nv = img.at<uchar>(ny,nx);
                    if(find(bridgeCenterValues.begin(), bridgeCenterValues.end(), nv)!=bridgeCenterValues.end()){
                        visited.at<uchar>(ny,nx)=1;
                        q.push(Point(nx,ny));
                    }
                }
            }

            // 判断连接的两个区域
            set<int> neighborRegions;
            for(auto &p:line){
                for(auto d:dirs4){
                    int nx=p.x+d.x, ny=p.y+d.y;
                    if(nx<0||ny<0||nx>=img.cols||ny>=img.rows) continue;
                    int rid = regionId.at<int>(ny,nx);
                    if(rid>=0) neighborRegions.insert(rid);
                }
            }

            if(neighborRegions.size()>=2){
                auto it = neighborRegions.begin();
                int rA = *it; ++it;
                int rB = *it;

                // 找到桥的端点（与区域相邻的点）
                Point startPt(-1,-1), endPt(-1,-1);
                for(auto &p:line){
                    for(auto d:dirs4){
                        int nx=p.x+d.x, ny=p.y+d.y;
                        if(nx<0||ny<0||nx>=img.cols||ny>=img.rows) continue;
                        int rid = regionId.at<int>(ny,nx);
                        if(rid == rA && startPt.x == -1) startPt = p;
                        if(rid == rB && endPt.x == -1) endPt = p;
                    }
                    if(startPt.x != -1 && endPt.x != -1) break;
                }

                bridges.push_back({line,rA,rB,v,startPt,endPt});
            }
        }
    }

    cout<<"桥数量: "<<bridges.size()<<endl;
    return bridges;
}

// -------------------- 优化的A*搜索（在区域内搜索）
struct AStarNode {
    int x, y;
    double f, g;
    bool operator>(const AStarNode& other) const { return f > other.f; }
};

vector<Point> AStarInRegionOptimized(const Mat& img, Point start, Point goal, int regionId, const Mat& regionMap) {
    int rows = img.rows, cols = img.cols;
    vector<vector<double>> g_score(rows, vector<double>(cols, 1e9));
    vector<vector<Point>> came_from(rows, vector<Point>(cols, Point(-1,-1)));
    vector<vector<uchar>> visited(rows, vector<uchar>(cols, 0));

    bool isBridge[256] = {0};
    for(auto v: bridgeCenterValues) isBridge[v] = true;

    vector<Point> dirs = {{1,0},{-1,0},{0,1},{0,-1}};
    priority_queue<AStarNode, vector<AStarNode>, greater<AStarNode>> openSet;

    g_score[start.y][start.x] = 0;
    double h = abs(goal.x - start.x) + abs(goal.y - start.y);
    openSet.push({start.x, start.y, h, 0});

    while(!openSet.empty()) {
        AStarNode current = openSet.top(); openSet.pop();
        int cx = current.x, cy = current.y;

        if(visited[cy][cx]) continue;
        visited[cy][cx] = 1;

        if(cx == goal.x && cy == goal.y) {
            vector<Point> path;
            Point node = goal;
            while(node != start) {
                path.push_back(node);
                node = came_from[node.y][node.x];
            }
            path.push_back(start);
            reverse(path.begin(), path.end());
            return path;
        }

        for(auto d: dirs) {
            int nx = cx + d.x;
            int ny = cy + d.y;
            if(nx<0||ny<0||nx>=cols||ny>=rows) continue;
            if(visited[ny][nx]) continue;

            uchar val = img.at<uchar>(ny,nx);
            bool canTraverse = (regionMap.at<int>(ny,nx) == regionId) || isBridge[val];
            if(!canTraverse) continue;

            double tentative_g = g_score[cy][cx] + 1;
            if(tentative_g < g_score[ny][nx]) {
                g_score[ny][nx] = tentative_g;
                came_from[ny][nx] = Point(cx, cy);
                double f = tentative_g + abs(goal.x - nx) + abs(goal.y - ny);
                openSet.push({nx, ny, f, tentative_g});
            }
        }
    }
    return {}; // 返回空路径
}

// -------------------- 全局路径规划
vector<Point> globalPathPlanning(const Mat& img, const Mat& regionId, 
                                const vector<BridgeCenter>& bridges, 
                                Point start, Point goal) {
    int startRegion = regionId.at<int>(start.y, start.x);
    int goalRegion = regionId.at<int>(goal.y, goal.x);

    if(startRegion == goalRegion) {
        return AStarInRegionOptimized(img, start, goal, startRegion, regionId);
    }

    vector<BridgeCenter> connectingBridges;
    for(const auto& bridge : bridges) {
        if((bridge.regionA == startRegion && bridge.regionB == goalRegion) ||
           (bridge.regionA == goalRegion && bridge.regionB == startRegion)) {
            connectingBridges.push_back(bridge);
        }
    }

    if(connectingBridges.empty()) {
        cout << "没有直接连接的桥梁，需要多区域路径规划" << endl;
        return {};
    }

    const auto& bridge = connectingBridges[0];
    vector<Point> fullPath;
    vector<Point> path1 = AStarInRegionOptimized(img, start, bridge.startPoint, startRegion, regionId);
    vector<Point> path2 = bridge.line;
    vector<Point> path3 = AStarInRegionOptimized(img, bridge.endPoint, goal, goalRegion, regionId);

    fullPath.insert(fullPath.end(), path1.begin(), path1.end());
    fullPath.insert(fullPath.end(), path2.begin(), path2.end());
    fullPath.insert(fullPath.end(), path3.begin(), path3.end());

    return fullPath;
}

// -------------------- 全局变量
Mat mapRaw, regionIdMat;
vector<Vec3b> regionColors;
vector<BridgeCenter> bridges;

// -------------------- 鼠标点击
Point startPt(-1,-1), endPt(-1,-1);
void onMouse(int event,int x,int y,int,void*){
    if(event==EVENT_LBUTTONDOWN){
        if(startPt.x<0) startPt=Point(x,y);
        else if(endPt.x<0) endPt=Point(x,y);
        if(startPt.x>=0 && endPt.x>=0){
            vector<Point> path = globalPathPlanning(mapRaw, regionIdMat, bridges, startPt, endPt);

            Mat vis; cvtColor(mapRaw,vis,COLOR_GRAY2BGR);
            for(int y=0;y<vis.rows;y++)
                for(int x=0;x<vis.cols;x++){
                    int id=regionIdMat.at<int>(y,x);
                    if(id>=0) vis.at<Vec3b>(y,x)=regionColors[id];
                }
            for(auto &b:bridges)
                for(auto &p:b.line) vis.at<Vec3b>(p.y,p.x)=Vec3b(0,0,255);
            for(auto &p:path)
                if(p.x>=0 && p.y>=0 && p.x<vis.cols && p.y<vis.rows)
                    vis.at<Vec3b>(p.y,p.x)=Vec3b(0,255,0);

            circle(vis, startPt, 3, Scalar(255,0,0), -1);
            circle(vis, endPt, 3, Scalar(0,255,255), -1);
            imshow("Path",vis);

            startPt = Point(-1,-1);
            endPt = Point(-1,-1);
        }
    }
}

// -------------------- main
int main(){
    mapRaw=imread("map.png",IMREAD_GRAYSCALE);
    if(mapRaw.empty()){ cout<<"无法读取地图\n"; return -1; }

    int regionCount;
    Mat regionVis=regionSegmentation(mapRaw, regionIdMat, regionCount, regionColors);

    bridges=detectBridges(mapRaw,regionIdMat);
    for(auto &b:bridges)
        for(auto &p:b.line)
            regionVis.at<Vec3b>(p.y,p.x)=Vec3b(0,0,255);

    imshow("Region Segmentation",regionVis);
    setMouseCallback("Region Segmentation",onMouse);
    cout << "点击设置起点和终点进行路径规划..." << endl;
    waitKey(0);
    return 0;
}
